#include "kinectsensor.h"
#include <QDebug>

// Safe release for interfaces
template<class Interface>
inline void SafeRelease(Interface *& pInterfaceToRelease)
{
    if (pInterfaceToRelease != NULL)
    {
        pInterfaceToRelease->Release();
        pInterfaceToRelease = NULL;
    }
}

KinectSensor::KinectSensor(QObject *parent) : QObject(parent)
{
    m_pKinectSensor=NULL;
    m_pCoordinateMapper=NULL;
    m_pBodyFrameReader=NULL;
    currentPBody=NULL;

    InitializeDefaultSensor();

    updatebody();

    currentMouseState=mouseState_no_state;


//        bitmap=new QBitmap("qrc:/imgSource/cursor.png");
    cursor=new QCursor(Qt::CrossCursor);
    cursor->setShape(Qt::DragMoveCursor);

    //update timer
    mTimer=new QTimer(this);
    connect(mTimer,SIGNAL(timeout()),this,SLOT(upDateKinect()));
    mTimer->start(30);

    //delete later
    connect(this,SIGNAL(destroyed(QObject*)),this,SLOT(closeKinect()));
}

HRESULT KinectSensor::InitializeDefaultSensor(){
    HRESULT hr;
    hr = GetDefaultKinectSensor(&m_pKinectSensor);
    if (FAILED(hr))
    {
        return hr;
    }

    if (m_pKinectSensor)
    {
        // Initialize the Kinect and get coordinate mapper and the body reader
        IBodyFrameSource* pBodyFrameSource = NULL;

        hr = m_pKinectSensor->Open();

        if (SUCCEEDED(hr))
        {
            hr = m_pKinectSensor->get_CoordinateMapper(&m_pCoordinateMapper);
        }

        if (SUCCEEDED(hr))
        {
            hr = m_pKinectSensor->get_BodyFrameSource(&pBodyFrameSource);
        }

        if (SUCCEEDED(hr))
        {
            hr = pBodyFrameSource->OpenReader(&m_pBodyFrameReader);
        }

        SafeRelease(pBodyFrameSource);
    }

    if (!m_pKinectSensor || FAILED(hr))
    {
        qDebug()<<"No ready Kinect found!";
        return E_FAIL;
    }

    return hr;

}


bool KinectSensor::getCurrentBody(IBody **ppbodies){
    for(int a=0;a<BODY_COUNT;a++){

        currentPBody=ppbodies[a];
        if(currentPBody){
            HRESULT hr;
            BOOLEAN btracked;
            hr=currentPBody->get_IsTracked(&btracked);
            if(SUCCEEDED(hr)&&btracked){
                return true;
            }
        }
    }
    return false;
}


void KinectSensor::getBodyJoint(){
    if(currentPBody){
        HRESULT hr;
        Joint joints[JointType_Count];
       hr=currentPBody->GetJoints(_countof(joints), joints);
       if(SUCCEEDED(hr)){
           leftHandJoint=joints[JointType_HandLeft];
           rightHandJoint=joints[JointType_HandRight];

           rightHandXChanged();
           rightHandYChanged();
       }
    }
}


void KinectSensor::updatebody(){

    if (!m_pBodyFrameReader)
    {
        return;
    }

    IBodyFrame* pBodyFrame = NULL;

    HRESULT hr = m_pBodyFrameReader->AcquireLatestFrame(&pBodyFrame);

    if (SUCCEEDED(hr))
    {
        INT64 nTime = 0;

        hr = pBodyFrame->get_RelativeTime(&nTime);

        //body position
        IBody* ppBodies[BODY_COUNT]={0};

        if (SUCCEEDED(hr))
        {
            hr = pBodyFrame->GetAndRefreshBodyData(_countof(ppBodies), ppBodies);
        }

        if (SUCCEEDED(hr))
        {
            //get body
            getCurrentBody(ppBodies);

            //get joint
            getBodyJoint();

            //map hand's position to coordinate position
            mapJointsToXYCoord();

            //preccess hands's states
            currentPBody->get_HandLeftState(&mleftHandState);
            currentPBody->get_HandRightState(&mrightHandState);
            leftHandStateChanged();
            rightHandStateChanged();
        }

        for (int i = 0; i < _countof(ppBodies); ++i)
        {
            SafeRelease(ppBodies[i]);
        }
    }

    SafeRelease(pBodyFrame);
}


bool KinectSensor::hasTrackingBody(){
    if(currentPBody){
        return true;
    }else{
        return false;
    }
}

void KinectSensor::mapJointsToXYCoord(){
    m_pKinectSensor->get_CoordinateMapper(&m_pCoordinateMapper);
    if(m_pCoordinateMapper){
        m_pCoordinateMapper->MapCameraPointToColorSpace(leftHandJoint.Position,&leftHandPoint);
        m_pCoordinateMapper->MapCameraPointToColorSpace(rightHandJoint.Position,&rightHandPoint);
        leftHandCoordXChanged();
        leftHandCoordYChanged();
        rightHandCoordXChanged();
        rightHandCoordYChanged();
        leftHandZChanged();
        leftHandXChanged();
        leftHandYChanged();
    }
}

/**
 * update timer slot
 **/
void KinectSensor::upDateKinect(){
    this->updatebody();
    if(mrightHandState!=HandState_NotTracked&&mrightHandState!=HandState_Lasso){
        cursor->setPos(rightHandPoint.X,rightHandPoint.Y);
        cursorxChanged();
        cursoryChanged();
    }

    //handle mouse event
    if(mrightHandState==HandState_Closed){
        if(currentMouseState==mouseState_no_state||currentMouseState==mouseState_released){
            sendMouseLeftPressEvent();
            currentMouseState=mouseState_pressing;
        }
        else if(currentMouseState==mouseState_pressing){
            sendMouseDragEvent();
            currentMouseState=mosueState_isdragging;
        }
        else if(currentMouseState==mosueState_isdragging){

        }
    }
    else if(mrightHandState==HandState_Open){
        if(currentMouseState==mouseState_pressing||currentMouseState==mosueState_isdragging){
            sendMouseLeftReleaseEvent();
            currentMouseState==mouseState_released;
        }
        else if(currentMouseState==mouseState_released){
            currentMouseState=mosueState_isdragging;
        }
    }
    else{
//        currentMouseState=mouseState_no_state;
        if(currentMouseState==mosueState_isdragging){
            sendMouseLeftReleaseEvent();
            currentMouseState=mouseState_no_state;
        }
        else{
            currentMouseState=mouseState_no_state;
        }
    }
}

/**
 * when application is closed,close kinect sensor
 * */
void KinectSensor::closeKinect(){
    m_pKinectSensor->Close();
}
